﻿using System.Runtime.InteropServices;
using MathS = System.Math;

#if F32
	using Num = System.Single;
	namespace Reign.Core.MathF32
#endif
#if F64
	using Num = System.Double;
	namespace Reign.Core.MathF64
#endif
#if I32
	using Num = System.Int32;
	namespace Reign.Core.MathI32
#endif
{
	[StructLayout(LayoutKind.Sequential)]
	public struct Vector3
	{
		#region Properties
		public Num X, Y, Z;
		public const int SizeOf = sizeof(Num) * 3;

		public Num R
		{
			get {return X;}
			set {X = value;}
		}

		public Num G
		{
			get {return Y;}
			set {Y = value;}
		}

		public Num B
		{
			get {return Z;}
			set {Z = value;}
		}

		// X
		public Vector2 XY
		{
			get {return new Vector2(X, Y);}
			set
			{
				X = value.X;
				Y = value.Y;
			}
		}

		public Vector2 XZ
		{
			get {return new Vector2(X, Z);}
			set
			{
				X = value.X;
				Z = value.Y;
			}
		}

		// Y
		public Vector2 YX
		{
			get {return new Vector2(Y, X);}
			set
			{
				Y = value.X;
				X = value.Y;
			}
		}

		public Vector2 YZ
		{
			get {return new Vector2(Y, Z);}
			set
			{
				Y = value.X;
				Z = value.Y;
			}
		}

		// Z
		public Vector2 ZX
		{
			get {return new Vector2(Z, X);}
			set
			{
				Z = value.X;
				X = value.Y;
			}
		}

		public Vector2 ZY
		{
			get {return new Vector2(Z, Y);}
			set
			{
				Z = value.X;
				Y = value.Y;
			}
		}

		// X Center
		public Vector3 YXZ
		{
			get {return new Vector3(Y, X, Z);}
			set
			{
				Y = value.X;
				X = value.Y;
				Z = value.Z;
			}
		}

		public Vector3 ZXY
		{
			get {return new Vector3(Z, X, Y);}
			set
			{
				Z = value.X;
				X = value.Y;
				Y = value.Z;
			}
		}

		// Y Center
		public Vector3 XYZ
		{
			get {return new Vector3(X, Y, Z);}
			set
			{
				X = value.X;
				Y = value.Y;
				Z = value.Z;
			}
		}

		public Vector3 ZYX
		{
			get {return new Vector3(Z, Y, X);}
			set
			{
				Z = value.X;
				Y = value.Y;
				X = value.Z;
			}
		}

		// Z Center
		public Vector3 XZY
		{
			get {return new Vector3(X, Z, Y);}
			set
			{
				X = value.X;
				Z = value.Y;
				Y = value.Z;
			}
		}

		public Vector3 YZX
		{
			get {return new Vector3(Y, Z, X);}
			set
			{
				Y = value.X;
				Z = value.Y;
				X = value.Z;
			}
		}
		#endregion

		#region Constructors
		public Vector3(Num value)
		{
			X = value;
			Y = value;
			Z = value;
		}

		public Vector3(Num x, Num y, Num z)
		{
			X = x;
			Y = y;
			Z = z;
		}

		public Vector3(Vector2 xy, Num z)
		{
			X = xy.X;
			Y = xy.Y;
			Z = z;
		}
		#endregion

		#region Operators
		// +
		public static Vector3 operator+(Vector3 p1, Vector3 p2) {return new Vector3(p1.X+p2.X, p1.Y+p2.Y, p1.Z+p2.Z);}
		public static Vector3 operator+(Vector3 p1, Num p2) {return new Vector3(p1.X+p2, p1.Y+p2, p1.Z+p2);}
		public static Vector3 operator+(Num p1, Vector3 p2) {return new Vector3(p1+p2.X, p1+p2.Y, p1+p2.Z);}
		// -
		public static Vector3 operator-(Vector3 p1, Vector3 p2) {return new Vector3(p1.X-p2.X, p1.Y-p2.Y, p1.Z-p2.Z);}
		public static Vector3 operator-(Vector3 p1, Num p2) {return new Vector3(p1.X-p2, p1.Y-p2, p1.Z-p2);}
		public static Vector3 operator-(Num p1, Vector3 p2) {return new Vector3(p1-p2.X, p1-p2.Y, p1-p2.Z);}
		public static Vector3 operator-(Vector3 p1) {return new Vector3(-p1.X, -p1.Y, -p1.Z);}
		// *
		public static Vector3 operator*(Vector3 p1, Vector3 p2) {return new Vector3(p1.X*p2.X, p1.Y*p2.Y, p1.Z*p2.Z);}
		public static Vector3 operator*(Vector3 p1, Num p2) {return new Vector3(p1.X*p2, p1.Y*p2, p1.Z*p2);}
		public static Vector3 operator*(Num p1, Vector3 p2) {return new Vector3(p1*p2.X, p1*p2.Y, p1*p2.Z);}
		// /
		public static Vector3 operator/(Vector3 p1, Vector3 p2) {return new Vector3(p1.X/p2.X, p1.Y/p2.Y, p1.Z/p2.Z);}
		public static Vector3 operator/(Vector3 p1, Num p2) {return new Vector3(p1.X/p2, p1.Y/p2, p1.Z/p2);}
		public static Vector3 operator/(Num p1, Vector3 p2) {return new Vector3(p1/p2.X, p1/p2.Y, p1/p2.Z);}
		// ==
		public static bool operator==(Vector3 p1, Vector3 p2) {return (p1.X==p2.X && p1.Y==p2.Y && p1.Z==p2.Z);}
		public static bool operator!=(Vector3 p1, Vector3 p2) {return (p1.X!=p2.X || p1.Y!=p2.Y || p1.Z!=p2.Z);}
		//Convert
		public Vector2 ToVector2() {return new Vector2(X, Y);}
		#endregion

		#region Methods
		public override bool Equals(object obj)
		{
			return obj != null && (Vector3)obj == this;
		}

		public override string ToString()
		{
			return string.Format("{0}, {1}, {2}", X, Y, Z);
		}

		public override int GetHashCode()
		{
			return base.GetHashCode();
		}

		public Vector3 Normal(Vector3 point1, Vector3 point2, Vector3 point3)
		{
			Vector3 vector1 = (point1-point2), vector2 = (point3-point1);
			return new Vector3(-((vector1.Y*vector2.Z) - (vector1.Z*vector2.Y)), -((vector1.Z*vector2.X) - (vector1.X*vector2.Z)), -((vector1.X*vector2.Y) - (vector1.Y*vector2.X))).Normalize();
		}

		public Vector3 Abs()
		{
			return new Vector3(MathS.Abs(X), MathS.Abs(Y), MathS.Abs(Z));
		}

		public Vector3 Pow(Num value)
		{
			return new Vector3((Num)MathS.Pow(X, value), (Num)MathS.Pow(Y, value), (Num)MathS.Pow(Z, value));
		}

		public Vector3 Floor()
		{
			#if I32
			return this;
			#else
			return new Vector3((Num)MathS.Floor(X), (Num)MathS.Floor(Y), (Num)MathS.Floor(Z));
			#endif
		}

		public Vector3 Max(Num value)
		{
			return new Vector3(MathS.Max(X, value), MathS.Max(Y, value), MathS.Max(Z, value));
		}

		public Vector3 Min(Num value)
		{
			return new Vector3(MathS.Min(X, value), MathS.Min(Y, value), MathS.Min(Z, value));
		}

		public Vector3 Cross(Vector3 vector)
		{
			return new Vector3(((Y*vector.Z) - (Z*vector.Y)), ((Z*vector.X) - (X*vector.Z)), ((X*vector.Y) - (Y*vector.X)));
		}

		public Vector3 Normalize()
		{
			return this * (Math.One / (Num)MathS.Sqrt((X*X) + (Y*Y) + (Z*Z)));
		}

		public Vector3 Normalize(out Num distance)
		{
			Num dis = (Num)MathS.Sqrt((X*X) + (Y*Y) + (Z*Z));
			distance = dis;
			return this * (Math.One/dis);
		}

		public Vector3 NormalizeSafe()
		{
			Num dis = (Num)MathS.Sqrt((X*X) + (Y*Y) + (Z*Z));
			if (dis == 0) return new Vector3();
			return this * (Math.One/dis);
		}

		public Vector3 NormalizeSafe(out Num distance)
		{
			Num dis = (Num)MathS.Sqrt((X*X) + (Y*Y) + (Z*Z));
			distance = dis;
			if (dis == 0) return new Vector3();
			return this * (Math.One/dis);
		}

		public Num Dot()
		{
			return (X*X) + (Y*Y) + (Z*Z);
		}

		public Num Dot(Vector3 vector)
		{
			return (X*vector.X) + (Y*vector.Y) + (Z*vector.Z);
		}

		public Num Distance()
		{
			return (Num)MathS.Sqrt((X*X) + (Y*Y) + (Z*Z));
		}
	
		public Num Angle(Vector3 vector)
		{
			Vector3 vec = this.Normalize();
			Num val = vec.Dot(vector.Normalize());
			val = (val > Math.One) ? Math.One : val;
			val = (val < -Math.One) ? -Math.One : val;
			return (Num)MathS.Acos(val);
		}
	
		public Vector3 Transform(Matrix3 matrix)
		{
			return (matrix.X*X) + (matrix.Y*Y) + (matrix.Z*Z);
		}

		public Vector3 Rotate(Vector3 vectorNormalized, Num radians)
		{
			throw new System.NotImplementedException();

			Vector3 y = new Vector3();
			/*if (pVectorNormalized.Dot() != 0)
			{
				y = this.InersectRay(pVectorNormalized) - this;
				if (y.Dot() != 0)
				{
					Matrix3f Matrix = new Matrix3f(pVectorNormalized, y);
					Matrix3f MatrixTran = Matrix.Transpose();
					Matrix = Matrix.RotateAroundAxisZ(pRadians);
					y = this.Transform(MatrixTran);
					y = y.Transform(Matrix);
				}
			}*/
			return y;
		}
	
		public bool WithinBound(Bound3 bound)
		{
			if (X >= bound.Left && X <= bound.Right && Y >= bound.Bottom && Y <= bound.Top && Z >= bound.Back && Z <= bound.Front) return true;
			return false;
		}

		public bool WithinTriangle(Vector3 p1, Vector3 p2, Vector3 p3)
		{      
			Vector3 v0 = p2 - p1;
			Vector3 v1 = p3 - p1;
			Vector3 v2 = this - p1;
			Num dot00 = v0.Dot();
			Num dot01 = v0.Dot(v1);
			Num dot02 = v0.Dot(v2);
			Num dot11 = v1.Dot();
			Num dot12 = v1.Dot(v2);
			Num invDenom = Math.One / ((dot00*dot11) - (dot01*dot01));
			Num u = ((dot11*dot02) - (dot01*dot12)) * invDenom;
			Num v = ((dot00*dot12) - (dot01*dot02)) * invDenom;
			return (u>0) && (v>0) && ((u+v) < Math.One);
		}
	
		public Vector3 Reflect(Vector3 planeNormal)
		{
			return this - (planeNormal * this.Dot(planeNormal) * Math.Two);
		}

		public Vector3 InersectNormal(Vector3 normal)
		{
			return (normal * this.Dot(normal));
		}

		public Vector3 InersectRay(Vector3 rayLocation, Vector3 rayNormal)
		{
			return (rayNormal * (this-rayLocation).Dot(rayNormal)) + rayLocation;
		}

		public Vector3 InersectPlane(Vector3 planeNormal, Vector3 planeLocation)
		{
			return this - (planeNormal * (this-planeLocation).Dot(planeNormal));
		}

		public Vector3 InersectLine(Line3 line)
		{
			Vector3 pointOffset = (this-line.P1), vector = (line.P2-line.P1).Normalize();
			return (vector * pointOffset.Dot(vector)) + line.P1;
		}

		/*public bool InersectTriangle(out Vector3f pInersectPoint, Vector3f pPolygonPoint1, Vector3f pPolygonPoint2, Vector3f pPolygonPoint3, Vector3f pPolygonNormal, Bound3D pPolygonBoundingBox, Vector3f pPoint)
		{
			pInersectPoint = pPoint.InersectPlane(pPolygonNormal, pPolygonPoint1);
			if (pInersectPoint.WithinTriangle(pPolygonBoundingBox) == false) return false;
			return Within(pPolygonPoint1, pPolygonPoint2, pPolygonPoint3);
		}*/

		public Vector3 UnProject(Matrix4 viewProjInverse, int viewX, int viewY, int viewWidth, int viewHeight)
		{
			var vec = new Vector4(this, 1);
			vec.X = (vec.X - viewX) / viewWidth;
			vec.Y = (vec.Y - viewY) / viewHeight;
			vec = (vec * 2) - 1;

			vec = vec.Transform(viewProjInverse);
			vec.X /= vec.W;
			vec.Y /= vec.W;
			vec.Z /= vec.W;
			
			return vec.ToVector3();
		}

		public Vector3 UnProject(Matrix4 projectionMatrix, Matrix4 viewMatrix, int viewX, int viewY, int viewWidth, int viewHeight)
		{
			var viewProjInverse = viewMatrix.Multiply(projectionMatrix).Invert();
			
			var vec = new Vector4(this, 1);
			vec.X = (vec.X - viewX) / viewWidth;
			vec.Y = (vec.Y - viewY) / viewHeight;
			vec = (vec * 2) - 1;

			vec = vec.Transform(viewProjInverse);
			vec.X /= vec.W;
			vec.Y /= vec.W;
			vec.Z /= vec.W;
			
			return vec.ToVector3();
		}

		public Vector3 Project(Matrix4 projectionMatrix, Matrix4 viewMatrix, int viewX, int viewY, int viewWidth, int viewHeight)
		{
			var vec = new Vector4(this, 1);
			vec = vec.Transform(viewMatrix);
			vec = vec.Transform(projectionMatrix);
			
			vec.X /= vec.W;
			vec.Y /= vec.W;
			vec.Z /= vec.W;
			
			vec.X = (vec.X * Math.Half) + Math.Half;
			vec.Y = (vec.Y * Math.Half) + Math.Half;
			vec.Z = (vec.Z * Math.Half) + Math.Half;

			vec.X = (vec.X * viewWidth) + viewX;
			vec.Y = (vec.Y * viewHeight) + viewY;

			return vec.ToVector3();
		}
		#endregion
	}
}